/*
 * Copyright (C) 2014, Field and Service Robotics 2014, Group 6.
 *
 *  Motion Ctrl
 */

#include "geometry_msgs/Twist.h"
#include "std_msgs/String.h"
#include "std_msgs/Int16.h"

#include "motionCtrl.h"
#include <math.h>


#include <stdlib.h>

void setVelParams()
{
	ROS_INFO("Setting parameters to server");

    ros::NodeHandle nh;

    //MotionCtrl velocities
    nh.setParam("approaching_linear_vel",           0.025); //0.03
    nh.setParam("approaching_rotational_vel",       0.07);  //0.1
    nh.setParam("turning_rotational_vel",           0.3); //0.5
    nh.setParam("normal_linear_vel",                0.15); //0.3

    //Basket approaching params
    nh.setParam("basket1_approaching_x",            0.15); //0.15
    nh.setParam("basket1_approaching_y",            -0.85); //-0.9
    nh.setParam("basket2_approaching_x",            3.0); //2.95
    nh.setParam("basket2_approaching_y",            -0.60); //-0.85
    nh.setParam("approaching_basket_linear_vel",     0.05);
    nh.setParam("approaching_basket_rotational_vel", 0.2);

    //Escape from basket params
    nh.setParam("escape_basket_linear",              -0.1); //-0.3
    nh.setParam("escape_basket_rotate",               1.6); //1.2

    //Escape from toy params
    nh.setParam("escape_toy_fixed_x",                 0); //robot will back off towards this point
    nh.setParam("escape_toy_fixed_y",                 0); //
    nh.setParam("escape_toy_distance",             -0.1); //-0.3 //distance to go backwards


    //For error situation handling
    nh.setParam("back_off_distance",             -0.1); //distance to go backwards in error situation

    //For time measurement
    nh.setParam("max_approach_wait_time",           40);

    //for testing
    //nh.setParam("debug",       0);
}


int main(int argc, char **argv)
{
	ros::init(argc, argv, "motionCtrl");
    ros::NodeHandle nodeHandle;
    setVelParams();
    
    MotionCtrl mc(nodeHandle);

	//Subscribers
    ros::Subscriber subStopMovement = nodeHandle.subscribe("stopMovement", 100, &MotionCtrl::stopMovementCmdReceived, &mc);
    ros::Subscriber subObstacleApproaching = nodeHandle.subscribe("obstacleApproaching", 100, &MotionCtrl::obstacleApproaching, &mc);
    ros::Subscriber subGoToPoint = nodeHandle.subscribe("goToPoint", 10, &MotionCtrl::goToPoint, &mc);


    ros::Rate loop_rate(100);

	while(ros::ok())
	{
		ros::spinOnce();
		loop_rate.sleep();		
	}
	for(int i = 0; i < 10; i++)
	{
		mc.stopMovement(MotionCtrl::EMERGENCY_STOPPED);
		loop_rate.sleep();
		ros::spinOnce();
	}

	return 0;
}

MotionCtrl::MotionCtrl(ros::NodeHandle& nh)
{
    mNodeHandle = nh;
    mCurrentState = STOPPED;
    mEmergencyStop = false;
    mCurrentApproachingState = IDLE;
    mApproachInterruptedLinear = false;
    mApproachInterruptedRotate = false;

    //publishers
    mVelPublisher = mNodeHandle.advertise<geometry_msgs::Twist>("RosAria/cmd_vel", 100); //REAL ROBOT
    //mVelPublisher = mNodeHandle.advertise<geometry_msgs::Twist>("cmd_vel", 100); //SIMULATION

    mApproachService = mNodeHandle.advertiseService("approach", &MotionCtrl::approachService, this);

    mStatePublisher = mNodeHandle.advertise<std_msgs::Int16>("motionCtrlState", 100);

    //mTriggerKinectClient= mNodeHandle.serviceClient<group6::kinectApproach>("toggleKinectPublish");
    mTriggerKinectClient= mNodeHandle.advertise<std_msgs::Bool>("toggleKinectPublish", 100);
}

bool MotionCtrl::approachService(group6::approach::Request &req, group6::approach::Response &res)
{
    if(req.question == APPROACH)
    {   
        /*
	if(!driveLinear(0.06, false))
	{
		res.result = NOT_SUCCEED;
		return true;
	}
       */
        mCurrentService = APPROACH;
        ROS_INFO("approach service!");
        res.result = approach();
        
        if(res.result == SUCCEED )
	{   
	    ROS_INFO("DRIVE BLINDLY APPROACH");
	    driveLinear(0.12, false);
	}  
  	
    }

    if(req.question == ESCAPE_FROM_BASKET)
    {
        mCurrentService = ESCAPE_FROM_BASKET;
        res.result = escapeFromBasket();
    }

    if(req.question == ESCAPE_FROM_TOY)
    {
        mCurrentService = ESCAPE_FROM_TOY;
        res.result = escapeFromToy();
    }

    if(req.question == BACKOFF)
    {
        mCurrentService = BACKOFF;
        res.result = backOff();
    }
    if(req.question == APPROACH_BASKET1 || req.question == APPROACH_BASKET2)
    {
        //mCurrentService = req.question;
        res.result = approachBasket(req.question);
    }

    return true;
}

MotionCtrl::status MotionCtrl::approach()
{
    //init for the new task
    mStartTime = ros::Time::now();
    //mStartTime = clock();
    mApproachInterruptedLinear = false;
    mApproachInterruptedRotate = false;
    mApproachDone = false;
    mErrorDuringApproach = false;
    mMaxTimeElapsedDuringApproach = false;
    mKinectCurrentResponse = KINECT_TOY_FOUND;
    mCurrentApproachingState = WAIT_FOR_MESSAGE;

    ROS_INFO("APPROACH STARTED");
    std_msgs::Bool boolToKinect;
    boolToKinect.data = true;
    mTriggerKinectClient.publish(boolToKinect);
    
    boolToKinect.data = false;
    
    //wait to get the approaching done
    ros::Rate loopRate(100);
    while(!mApproachDone)
    {
        if(mKinectCurrentResponse == KINECT_TOY_NOT_FOUND)
        {
            mTriggerKinectClient.publish(boolToKinect);
            return ERROR_TOY_NOT_FOUND;
        }
        if(mErrorDuringApproach)
        {
            mTriggerKinectClient.publish(boolToKinect);
            return NOT_SUCCEED;
        }
        if(mMaxTimeElapsedDuringApproach)
        {
            mTriggerKinectClient.publish(boolToKinect);
            return MAX_TIME_EXCEEDED;
        }
        ros::spinOnce();
        loopRate.sleep();
    }
    
    
    boolToKinect.data = false;
    mTriggerKinectClient.publish(boolToKinect);

    return SUCCEED;

}



MotionCtrl::status MotionCtrl::escapeFromBasket()
{
    bool succeed = true;

    double distance;
    double angle;
    mNodeHandle.getParam("escape_basket_linear",distance);
    mNodeHandle.getParam("escape_basket_rotate",angle);

    if (distance != 0){
    succeed = driveLinear(distance, false);
    }

    if(angle != 0){
        succeed = rotate(angle, false);
    }

    if(succeed)
    {
        return SUCCEED;
    }
    else
    {
        return NOT_SUCCEED;
    }
}


//Robot will drive backwards towards point declared in
// ("escape_toy_fixed_x", "escape_toy_fixed_y")
MotionCtrl::status MotionCtrl::escapeFromToy()
{
    double distanceToBackOff;
    mNodeHandle.getParam("escape_toy_distance", distanceToBackOff);
    if(!driveLinear(distanceToBackOff, false))
    {
        return NOT_SUCCEED;
    }
    return SUCCEED;

    /* SPIN TOWARDS POINT AND THEN BACK OFF IMPLEMENTATION
     * USED IN MID TERM 2, NOT SUITABLE FOR FINAL
    //wait for the listener to get the first message
    mListener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(1));
    //we will record transforms here
    tf::StampedTransform currentTransform;
    if(!getCurrentTransform(currentTransform))
    {
        return NOT_SUCCEED;
    }

    //Calculate needed params
    double angleToBackOffPoint = getAngleToBackOffPoint(currentTransform);
    double distanceToBackOff;
    mNodeHandle.getParam("escape_toy_distance", distanceToBackOff);


    //Actual back off from toy
    if(rotate(angleToBackOffPoint,false) && driveLinear(distanceToBackOff, false))
    {
    return SUCCEED;
    }

    return NOT_SUCCEED;
    */
}

MotionCtrl::status MotionCtrl::backOff()
{
    double distance;
    mNodeHandle.getParam("back_off_distance", distance);
    if(!driveLinear(distance, false))
    {
        return NOT_SUCCEED;
    }
    return SUCCEED;
}

MotionCtrl::status MotionCtrl::approachBasket(int service)
{
    //wait for the listener to get the first message
    mListener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(1));
    //we will record transforms here
    tf::StampedTransform currentTransform;
    if(!getCurrentTransform(currentTransform))
    {
        return NOT_SUCCEED;
    }

    double wantedAngle;
    double deltaLinear;
    //mNodeHandle.getParam("basket_angle", wantedAngle);
    double currentAngle = tf::getYaw(currentTransform.getRotation());
    //double angleToTurn = wantedAngle-currentAngle;

    double basket_approaching_x;
    double basket_approaching_y;

    if(service == APPROACH_BASKET1)
    {
        mCurrentService = APPROACH_BASKET1;
        mNodeHandle.getParam("basket1_angle", wantedAngle);
        mNodeHandle.getParam("basket1_approaching_x", basket_approaching_x);
        mNodeHandle.getParam("basket1_approaching_y", basket_approaching_y);
        deltaLinear = fabs(currentTransform.getOrigin().y()-basket_approaching_y);
    }
    else if (service == APPROACH_BASKET2)
    {
        mCurrentService = APPROACH_BASKET2;
        mNodeHandle.getParam("basket2_angle", wantedAngle);
        mNodeHandle.getParam("basket2_approaching_x", basket_approaching_x);
        mNodeHandle.getParam("basket2_approaching_y", basket_approaching_y);
        deltaLinear = fabs(currentTransform.getOrigin().x()-basket_approaching_x);
    }
    else
    {
        return NOT_SUCCEED; // invalid service call
    }
    double angleToTurn = wantedAngle-currentAngle;

    //For mid term 2 only the y distance affects, so that's why the x coordinate is not taken into consideration
    //Maybe some more sophisticated solution has to be done in the future
    //double deltaLinear = currentTransform.getOrigin().y()-basket_approaching_y;

    if(!rotate(angleToTurn, true) || !driveLinear(deltaLinear, true))
    {
        return NOT_SUCCEED;
    }

    return SUCCEED;
}


double MotionCtrl::getAngleToBackOffPoint(tf::StampedTransform& currentTransform)
{
    double currentAngle = tf::getYaw(currentTransform.getRotation());
    ROS_INFO("currentAngle %f", currentAngle);
    double targetX;
    double targetY;
    mNodeHandle.getParam("escape_toy_fixed_x", targetX);
    mNodeHandle.getParam("escape_toy_fixed_y", targetY);

    double deltaX  = targetX - currentTransform.getOrigin().x();
    double deltaY = targetY - currentTransform.getOrigin().y();

    double angle = atan(deltaY/deltaX);

    ROS_INFO("deltaX: %f deltaY: %f  Angle: %f", deltaX, deltaY,angle);

    return angle - currentAngle;


}


void MotionCtrl::obstacleApproaching(const std_msgs::Bool::ConstPtr& boolean){
    ROS_INFO("Motion Ctrl obstacle approaching");
    if(mCurrentState == SPINNING)
    {
        return; // ignore obstacle approaching messages in SPINNING, IS THIS SAVE? /jerry
    }
    mEmergencyStop = true;
    //stopMovement(); I dont not whether this should be called here or in actual driving functions?? /jerry
}

void MotionCtrl::stopMovementCmdReceived(const std_msgs::Bool::ConstPtr& boolean)
{
    ROS_INFO("Motion Ctrl stopMovement command received");
    if(boolean->data)
    {
        stopMovement(MotionCtrl::STOPPED);
    }
}

void MotionCtrl::stopMovement(enum currentStates state)
{
    geometry_msgs::Twist stopMsg;
    mVelPublisher.publish(stopMsg);
    mCurrentState = MotionCtrl::STOPPED;
    //sendCurrentState(state);
}



void MotionCtrl::sendCurrentState(const currentStates& currentState){
	std_msgs::Int16 state;
	state.data = currentState;
    mStatePublisher.publish(state);
}


//! Drive forward a specified distance based on odometry information
bool MotionCtrl::driveLinear(double distance,  bool approaching)
{
    ROS_INFO("driveLinear distance = %f, approaching = %d", distance, approaching);
    mEmergencyStop = false; //have to clear this one, it will pop up before going forward if there still is danger
    mCurrentState = MOVING_FORWARD;
    sendCurrentState(mCurrentState);

    //wait for the listener to get the first message
    mListener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(1));

    //we will record transforms here
    tf::StampedTransform startTransform;
    tf::StampedTransform currentTransform;

    //record the starting transform from the odometry to the base frame
    mListener.lookupTransform("/map","/base_link",ros::Time(0), startTransform);

    geometry_msgs::Twist baseCmd;

    if(mCurrentService == APPROACH)
    {
        mNodeHandle.getParam("approaching_linear_vel", baseCmd.linear.x);
    }
    else if (mCurrentService == APPROACH_BASKET1 || mCurrentService == APPROACH_BASKET2)
    {
        mNodeHandle.getParam("approaching_basket_linear_vel", baseCmd.linear.x);
    }
    else
    {
        mNodeHandle.getParam("normal_linear_vel", baseCmd.linear.x);
    }

    //debug
    ROS_INFO("Linear velocity: %f", baseCmd.linear.x);

    if(distance < 0)
    {
        baseCmd.linear.x *= -1; //Going backwards
    }

    ros::Rate rate(10.0);
    bool done = false;
    while (!done && mNodeHandle.ok())
    {
        ros::spinOnce(); //have to spin once to receive subscribed messages inside loop

        // Check if the approaching is interrupted due to message from kinect
        if(mApproachInterruptedLinear)
        {
            ROS_INFO("Approaching interrupted while driving linear.");
            stopMovement(APPROACH_INTERRUPTED);
            mApproachInterruptedLinear = false;
            if(mCurrentService == APPROACH)
            {
                mCurrentApproachingState = WAIT_FOR_MESSAGE;
            }
            return true;
        }

        if(mEmergencyStop)
        {
            stopMovement(MotionCtrl::EMERGENCY_STOPPED);
            ROS_INFO("EMERGENCY STOPPED WHILE DRIVING FORWARD");
            return false;
        }

        //send the drive command
        mVelPublisher.publish(baseCmd);
        rate.sleep();

        if(!getCurrentTransform(currentTransform))
        {
            break;
        }
        
        double dist_moved = sqrt(
        pow((currentTransform.getOrigin().x() - startTransform.getOrigin().x()), 2)+
        pow((currentTransform.getOrigin().y() - startTransform.getOrigin().y()), 2));
        
       // ROS_INFO("dist moved %f", dist_moved);

        if(dist_moved >= fabs(distance))
        {
            done = true;
            ROS_INFO("DONE!!");
        }
    }
    //stop movement
    baseCmd.linear.x = 0;
    mVelPublisher.publish(baseCmd);
    if (done)
    {
        if(mCurrentService == APPROACH)
        {
            mCurrentApproachingState = WAIT_FOR_MESSAGE;
        }
        ROS_INFO("REACHED GOAL WHILE DRIWING LINEAR");
        return true;
    }
    ROS_INFO("COULD NOT REACH THE GOAL WHILE DRIVING LINEAR");
    return false;
    
}


bool MotionCtrl::rotate(double radians, bool approaching)
 {
    ROS_INFO("rotate angle = %f, approaching = %d", radians, approaching);
    mCurrentState = SPINNING;
    sendCurrentState(mCurrentState);

    //wait for the listener to get the first message
    mListener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(1));

    //we will record transforms here
    tf::StampedTransform startTransform;
    tf::StampedTransform currentTransform;

    //record the starting transform from the odometry to the base frame
    mListener.lookupTransform("/map","/base_link",ros::Time(0), startTransform);

    //ROS_INFO("Start transform rotation: %f", tf::getYaw(startTransform.getRotation())/*startTransform.getRotation().z()*/);

    //we will be sending commands of type "twist"
    geometry_msgs::Twist baseCmd;

    //check if we are approaching (slow motion) or just turning towards basket or next toy (fast motion)
    if(mCurrentService == APPROACH)
    {
        mNodeHandle.getParam("approaching_rotational_vel", baseCmd.angular.z);
    }
    else if(mCurrentService == APPROACH_BASKET1 || mCurrentService == APPROACH_BASKET2)
    {
        mNodeHandle.getParam("approaching_basket_rotational_vel", baseCmd.angular.z);
    }
    else
    {
        mNodeHandle.getParam("turning_rotational_vel", baseCmd.angular.z);
    }

    //debug
    ROS_INFO("Current rotational velocity: %f", baseCmd.angular.z);

    if (radians < 0)
    {
        radians *= -1;
        baseCmd.angular.z = -baseCmd.angular.z;
    }


    ros::Rate rate(10.0);
    bool done = false;
    while (!done && mNodeHandle.ok())
    {
        ros::spinOnce(); //have to spin once to receive subscribed messages inside loop

        // Check if the approaching is interrupted due to message from kinect
        if(mApproachInterruptedRotate)
        {
            ROS_INFO("Approaching interrupted while rotate.");
            stopMovement(APPROACH_INTERRUPTED);
            mApproachInterruptedRotate = false;
            if(mCurrentService == APPROACH)
            {
                mCurrentApproachingState = WAIT_FOR_MESSAGE;
            }
            return true;
        }

        //send the drive command
        mVelPublisher.publish(baseCmd);
        rate.sleep();


        if(!getCurrentTransform(currentTransform))
        {
            break;
        }

        //ROS_INFO("Current rotation: %f", tf::getYaw(currentTransform.getRotation())/*currentTransform.getRotation().z()*/);

        double angleTurned = calculateAngleDifference(tf::getYaw(currentTransform.getRotation()), tf::getYaw(startTransform.getRotation()));
       // ROS_INFO("angle turned %f", angleTurned);

        if (angleTurned >= radians)
        {
            done = true;
        }
    }

    //Stop rotation
    baseCmd.angular.z = 0;
    mVelPublisher.publish(baseCmd);

    if (done)
    {
        if(mCurrentService == APPROACH)
        {
            mCurrentApproachingState = WAIT_FOR_MESSAGE;
        }

        ROS_INFO("REACHED THE GOAL WHILE ROTATE");
        return true;
    }
    ROS_INFO("COULD NOT REACH THE GOAL WHILE ROTATE");
    return false;
    
}

bool MotionCtrl::checkIfMaxApproachTime()
{
    ros::Duration delta = ros::Time::now() - mStartTime;

    double maxWait;

    if(!mNodeHandle.getParam("max_approach_wait_time", maxWait))
    {
        ROS_ERROR("Error while getting max_approach_wait_time");
        return true;
    }

    //debug
    ROS_INFO("Time elased delta: %f   maxWait: %f", delta.toSec(), maxWait);

    return delta.toSec() > maxWait;

    /*
    clock_t current = clock();
    ROS_INFO("START %zu  CURRENT %zu ", mStartTime, current);
    ROS_INFO("Time elapsed %zu", (current - mStartTime)  );
    return ((current - mStartTime) > 400000); 
    */
}

void MotionCtrl::goToPoint(const group6::motion_ctrl::ConstPtr& msg)
{

   ROS_INFO("Motion Ctrl received motion_ctrl message: %f %f %d", msg->distance, msg->angle, msg->status);

    if(msg->status == KINECT_TOY_NOT_FOUND && (mCurrentApproachingState != IN_PROGRESS_LINEAR &&
                                               mCurrentApproachingState != IN_PROGRESS_ROTATE))
    {
        ROS_INFO("ERROR IN KINECT STATUS! NO TOY IN VIEW!");
        mKinectCurrentResponse = KINECT_TOY_NOT_FOUND;
    }


    //Added for stopping linear with distance == 0 message
    else if(mCurrentApproachingState == IN_PROGRESS_LINEAR && !mApproachDone
            && msg->distance == 0)
    {
        mApproachInterruptedLinear = true;
    }


    //Added from stopping rotate with angle == 0 message
    else if(mCurrentApproachingState == IN_PROGRESS_ROTATE && !mApproachDone
            && msg->angle == 0)
    {
        mApproachInterruptedRotate = true;
    }


    /* REMOVED BECAUSE STOPPED MOVEMENT ONLY WITH distance == 0 && angle == 0 message
     *
    // Approaching interrupt because of msg->angle == 0 && msg->distance == 0
    // while the movement is not finished -> have to stop
    else if(mCurrentApproachingState == IN_PROGRESS && !mApproachDone
            && msg->distance == 0 && msg->angle == 0)
    {
        mApproachInterrupted = true;
        mCurrentApproachingState = IDLE;
        mApproachDone = true;
        return;
    }
    */

    else if(mCurrentApproachingState == WAIT_FOR_MESSAGE && !mApproachDone)
    {
        if(checkIfMaxApproachTime()){
            ROS_ERROR("APPROACHING EXCEEDED MAX TIME LIMIT");
            mMaxTimeElapsedDuringApproach = true;
            mCurrentApproachingState = IDLE;
            return;
        }
        
        //ROS_INFO("time to move!");
        //mCurrentApproachingState = IN_PROGRESS;

        //First we want to get to the correct angle
        if(msg->angle != 0)
        {
            mCurrentApproachingState = IN_PROGRESS_ROTATE;
            //ROS_INFO("time to rotate");
            if(!rotate(msg->angle, true))
            {
                mErrorDuringApproach = true;
                mCurrentApproachingState = IDLE;
            }
        }

        //Only if the angle is correct we do the linear movement
        else if (msg->distance != 0 && !mApproachDone)
        {
            mCurrentApproachingState = IN_PROGRESS_LINEAR;
            //ROS_INFO("time to drive linear");
            if(!driveLinear(msg->distance, true))
            {
                mErrorDuringApproach = true;
                mCurrentApproachingState = IDLE;
            }

        }
        else // msg->angle == 0 && msg->distance == 0
        {
            ROS_INFO("APPROACHING DONE!");
            mApproachDone = true;
            mCurrentApproachingState = IDLE;
        }
    }
    else
    {
        //ROS_INFO("skip received message");
    }

}




/* Calculates difference between two angles
 * Works well if the difference is less than pi (half of the circle),
 * we won't need turns that are more than that
 */
double MotionCtrl::calculateAngleDifference(const double &current, const double &original)
{
    double difference = current - original;
    while (difference < -M_PI)
    {
        difference += 2*M_PI;
    }
    while (difference > M_PI)
    {
        difference -= 2*M_PI;
    }
    return fabs(difference);
}

bool MotionCtrl::getCurrentTransform(tf::StampedTransform &currentTransform)
{
    try
    {
        mListener.lookupTransform("/map","/base_link",ros::Time(0), currentTransform);
    }
    catch (tf::TransformException ex)
    {
        ROS_ERROR("%s",ex.what());
        return false;
    }
    return true;
}





